# from spirems import Subscriber, sms2cvimg
from spirems import Publisher, Subscriber, cvimg2sms, sms2cvimg, def_msg
import cv2
import rclpy
from rclpy.node import Node
import argparse
from std_msgs.msg import Float64MultiArray
from a2rl_bs_msgs.msg import VectornavIns, CartesianFrame, Localization
from rosgraph_msgs.msg import Clock
from geometry_msgs.msg import PoseStamped, Vector3, Quaternion
from sensor_msgs.msg import PointCloud2, NavSatFix
from vectornav_msgs.msg import AttitudeGroup, CommonGroup, GpsGroup, ImuGroup
import threading
from typing import Union
import argparse
import os
from spirems import Publisher, Subscriber, def_msg, cvimg2sms, BaseNode, get_extra_args


"""
-------------------------------------------------------------------
SpireMS                            -> ROS2 Message
-------------------------------------------------------------------
nav_msgs::VectornavIns             -> VectornavIns
std_msgs::Clock                    -> Clock
geometry_msgs::PoseInFrame         -> PoseStamped
memory_msgs::PointCloud            -> PointCloud2
sensor_msgs::NavSatFix             -> NavSatFix
nav_msgs::VectornavAttitudeGroup   -> AttitudeGroup
nav_msgs::VectornavCommonGroup     -> CommonGroup
nav_msgs::VectornavGpsGroup        -> GpsGroup
nav_msgs::VectornavImuGroup        -> ImuGroup
geometry_msgs::FrameTransform      -> TFMessage
geometry_msgs::Twist               -> TwistStamped
sensor_msgs::Imu                   -> Imu
nav_msgs::MahalanobisData          -> MahalanobisData
vehicle_msgs::ModuleStatusReport   -> ModuleStatusReport
nav_msgs::Odometry                 -> Odometry
nav_msgs::Localization             -> Localization
vehicle_msgs::EgoState             -> EgoState
geometry_msgs::PoseWithCovariance  -> PoseWithCovarianceStamped
geometry_msgs::TwistWithCovariance -> TwistWithCovarianceStamped
std_msgs::Boolean                  -> Bool
vehicle_msgs::KistlerCorrevit      -> Kistler_Correvit
-------------------------------------------------------------------
"""


class SMSA2RLTopic2ROS2Node(Node, threading.Thread, BaseNode):
    def __init__(
        self,
        job_name: str,
        ip: str = '127.0.0.1',
        port: int = 9094,
        param_dict_or_file: Union[dict, str] = None,
        sms_shutdown: bool = True,
        **kwargs
    ):
        super().__init__('SMSA2RLTopic2ROS2Node')
        threading.Thread.__init__(self)
        sms_shutdown = True if sms_shutdown in ['True', 'true', '1', True] else False
        BaseNode.__init__(
            self,
            self.__class__.__name__,
            job_name,
            ip=ip,
            port=port,
            param_dict_or_file=param_dict_or_file,
            sms_shutdown=sms_shutdown,
            **kwargs
        )

        # self.ros_topic = self.get_param("ros_topic", "/sensor/lidar_front/points")
        self.params_help()

        self.sms_a2rl_vn_ins_sub = Subscriber('/a2rl/vn/ins', 'std_msgs::Null', self.a2rl_vn_ins_callback)
        self.sms_clock_sub = Subscriber('/clock', 'std_msgs::Null', self.clock_callback)
        self.sms_flyeagle_ground_truth_sub = Subscriber('/flyeagle/ground_truth', 'std_msgs::Null', self.flyeagle_ground_truth_callback)
        self.sms_lidar_front_sub = Subscriber('/flyeagle/lidar_front', 'memory_msgs::PointCloud', self.lidar_front_callback)
        self.sms_vectornav_nav_sat_fix_sub = Subscriber('/vectornav/nav_sat_fix', 'std_msgs::Null', self.vectornav_nav_sat_fix_callback)
        self.sms_vectornav_raw_attitude_sub = Subscriber('/vectornav/raw/attitude', 'std_msgs::Null', self.vectornav_raw_attitude_callback)
        self.sms_vectornav_raw_common_sub = Subscriber('/vectornav/raw/common', 'std_msgs::Null', self.vectornav_raw_common_callback)
        self.sms_vectornav_raw_gps_sub = Subscriber('/vectornav/raw/gps', 'std_msgs::Null', self.vectornav_raw_gps_callback)
        self.sms_vectornav_raw_gps2_sub = Subscriber('/vectornav/raw/gps2', 'std_msgs::Null', self.vectornav_raw_gps2_callback)
        self.sms_vectornav_raw_imu_sub = Subscriber('/vectornav/raw/imu', 'std_msgs::Null', self.vectornav_raw_imu_callback)

        # ning
        # self.sms_flyeagle_a2rl_observer_ego_loc_sub = Subscriber('/flyeagle/a2rl/observer/ego_loc', 'std_msgs::Null', self.flyeagle_a2rl_observer_ego_loc_callback)


        self.a2rl_vn_ins_pub = self.create_publisher(VectornavIns, '/a2rl/vn/ins', 10)
        self.clock_pub = self.create_publisher(Clock, '/clock', 10)
        self.flyeagle_ground_truth_pub = self.create_publisher(PoseStamped, '/flyeagle/ground_truth', 10)
        self.lidar_front_pub = self.create_publisher(PointCloud2, '/flyeagle/lidar_front/points', 10)
        self.vectornav_nav_sat_fix_pub = self.create_publisher(NavSatFix, '/vectornav/nav_sat_fix', 10)
        self.vectornav_raw_attitude_pub = self.create_publisher(AttitudeGroup, '/vectornav/raw/attitude', 10)
        self.vectornav_raw_common_pub = self.create_publisher(CommonGroup, '/vectornav/raw/common', 10)
        self.vectornav_raw_gps_pub = self.create_publisher(GpsGroup, '/vectornav/raw/gps', 10)
        self.vectornav_raw_gps2_pub = self.create_publisher(GpsGroup, '/vectornav/raw/gps2', 10)
        self.vectornav_raw_imu_pub = self.create_publisher(ImuGroup, '/vectornav/raw/imu', 10)
        # ning
        # self.flyeagle_a2rl_observer_ego_loc_pub = self.create_publisher(Localization, '/flyeagle/a2rl/observer/ego_loc', 10)

    def a2rl_vn_ins_callback(self, sms_msg):
        rosmsg = VectornavIns()        
        rosmsg.timestamp.nanoseconds = int(sms_msg['timestamp'] * 1e9)
        rosmsg.time_gps.nanoseconds = int(sms_msg['time_gps'] * 1e9)
        rosmsg.ins_status = sms_msg['ins_status']
        rosmsg.acceleration_ins = CartesianFrame(x=sms_msg['acceleration_ins']['x'], y=sms_msg['acceleration_ins']['y'], z=sms_msg['acceleration_ins']['z'])
        rosmsg.angular_rate_ins = CartesianFrame(x=sms_msg['angular_rate_ins']['x'], y=sms_msg['angular_rate_ins']['y'], z=sms_msg['angular_rate_ins']['z'])
        rosmsg.position_enu_ins = CartesianFrame(x=sms_msg['position_enu_ins']['x'], y=sms_msg['position_enu_ins']['y'], z=sms_msg['position_enu_ins']['z'])
        rosmsg.velocity_enu_ins = CartesianFrame(x=sms_msg['velocity_enu_ins']['x'], y=sms_msg['velocity_enu_ins']['y'], z=sms_msg['velocity_enu_ins']['z'])        
        rosmsg.velocity_body_ins = CartesianFrame(x=sms_msg['velocity_body_ins']['x'], y=sms_msg['velocity_body_ins']['y'], z=sms_msg['velocity_body_ins']['z'])
        rosmsg.pos_stddev_ins = sms_msg['pos_stddev_ins']
        rosmsg.vel_stddev_ins = sms_msg['vel_stddev_ins']
        rosmsg.orientation_ypr = CartesianFrame(x=sms_msg['orientation_ypr']['x'], y=sms_msg['orientation_ypr']['y'], z=sms_msg['orientation_ypr']['z'])
        rosmsg.orientation_stddev = CartesianFrame(x=sms_msg['orientation_stddev']['x'], y=sms_msg['orientation_stddev']['y'], z=sms_msg['orientation_stddev']['z'])
        # rate = sms_converter.create_rate(10)
        self.a2rl_vn_ins_pub.publish(rosmsg)
        # rate.sleep()
    
    def clock_callback(self, sms_msg):
        rosmsg = Clock()
        rosmsg.clock.sec = int(sms_msg['timestamp'])
        rosmsg.clock.nanosec = int((sms_msg['timestamp']-int(sms_msg['timestamp'])) * 1e9)

        self.clock_pub.publish(rosmsg)

    def flyeagle_ground_truth_callback(self, sms_msg):
        rosmsg = PoseStamped()
        rosmsg.header.stamp.sec = int(sms_msg['timestamp'])
        rosmsg.header.stamp.nanosec = int((sms_msg['timestamp']-int(sms_msg['timestamp'])) * 1e9)
        rosmsg.pose.position.x = sms_msg['pose']['position']['x']

        rosmsg.pose.position.y = sms_msg['pose']['position']['y']
        rosmsg.pose.position.z = sms_msg['pose']['position']['z']
        rosmsg.pose.orientation.x = sms_msg['pose']['orientation']['x']
        rosmsg.pose.orientation.y = sms_msg['pose']['orientation']['y']
        rosmsg.pose.orientation.z = sms_msg['pose']['orientation']['z']
        rosmsg.pose.orientation.w = sms_msg['pose']['orientation']['w']

        self.flyeagle_ground_truth_pub.publish(rosmsg)

    def lidar_front_callback(self, sms_msg):
        rosmsg = PointCloud()
        rosmsg.header.stamp.sec = int(sms_msg['timestamp'])
        rosmsg.header.stamp.nanosec = int((sms_msg['timestamp']-int(sms_msg['timestamp'])) * 1e9)
        rosmsg.header.frame_id = sms_msg['frame_id']
        rosmsg.height = sms_msg['height']
        rosmsg.width = sms_msg['width']
        
        t1 = time.time()

        # 从 SMS 消息中提取点云数据和元信息
        # 假设 sms_msg['data'] 是点云数组，形状为 (N, 6)
        pcd_ = sms_msg['data']  # NumPy 数组，包含 [intensity, x, y, z, scan_idx, ring]
        frame_id = sms_msg.get('frame_id', 'lidar_front')  # 默认帧 ID
        timestamp = sms_msg.get('timestamp', time.time())  # 时间戳（秒）

        # 确保数据类型正确
        pcd_ = pcd_.astype(np.float32)  # x, y, z, intensity 为 float32
        pcd_[:, [4, 5]] = pcd_[:, [4, 5]].astype(np.int32).astype(np.float32)  # scan_idx, ring 为 int32

        # 创建 PointCloud2 消息
        ros2_msg = PointCloud2()

        # 设置 header
        ros2_msg.header = Header()
        ros2_msg.header.frame_id = frame_id
        ros2_msg.header.stamp.sec = int(timestamp)
        ros2_msg.header.stamp.nanosec = int((timestamp - int(timestamp)) * 1e9)

        # 设置点云尺寸
        ros2_msg.height = 1  # 无结构点云
        ros2_msg.width = pcd_.shape[0]  # 点数

        # 定义字段
        fields = [
            PointField(name='x', offset=4, datatype=PointField.FLOAT32, count=1),
            PointField(name='y', offset=8, datatype=PointField.FLOAT32, count=1),
            PointField(name='z', offset=12, datatype=PointField.FLOAT32, count=1),
            PointField(name='intensity', offset=0, datatype=PointField.FLOAT32, count=1),
            PointField(name='scan_idx', offset=16, datatype=PointField.INT32, count=1),
            PointField(name='ring', offset=20, datatype=PointField.INT32, count=1),
        ]
        ros2_msg.fields = fields

        # 设置字节序和步长
        ros2_msg.is_bigendian = False
        ros2_msg.point_step = 24  # 每个点的字节数：4*4 (float32) + 4*2 (int32) = 24
        ros2_msg.row_step = ros2_msg.point_step * ros2_msg.width  # 总字节数

        # 将点云数据转为二进制
        # 调整字段顺序以匹配 [intensity, x, y, z, scan_idx, ring]
        reordered_pcd = np.zeros_like(pcd_)
        reordered_pcd[:, 0] = pcd_[:, 1]  # x
        reordered_pcd[:, 1] = pcd_[:, 2]  # y
        reordered_pcd[:, 2] = pcd_[:, 3]  # z
        reordered_pcd[:, 3] = pcd_[:, 0]  # intensity
        reordered_pcd[:, 4] = pcd_[:, 4]  # scan_idx
        reordered_pcd[:, 5] = pcd_[:, 5]  # ring

        ros2_msg.data = reordered_pcd.tobytes()

        # 设置稠密标志
        ros2_msg.is_dense = True  # 假设没有无效点

        # 发布消息
        self.ros2_lidar_front_pub.publish(ros2_msg)

    def vectornav_nav_sat_fix_callback(self, sms_msg):
        rosmsg = NavSatFix()
        rosmsg.header.stamp.sec = int(sms_msg['timestamp'])
        rosmsg.header.stamp.nanosec = int((sms_msg['timestamp']-int(sms_msg['timestamp'])) * 1e9)
        rosmsg.status.status = sms_msg['status'][0]
        rosmsg.status.service = sms_msg['status'][1]
        rosmsg.latitude = sms_msg['latitude']
        rosmsg.longitude = sms_msg['longitude']
        rosmsg.altitude = sms_msg['altitude']
        rosmsg.position_covariance = sms_msg['position_covariance']
        rosmsg.position_covariance_type = sms_msg['position_covariance_type']

        self.vectornav_nav_sat_fix_pub.publish(rosmsg)
    
    def vectornav_raw_attitude_callback(self, sms_msg):
        rosmsg = AttitudeGroup()
        rosmsg.header.stamp.sec = int(sms_msg['timestamp'])
        rosmsg.header.stamp.nanosec = int((sms_msg['timestamp']-int(sms_msg['timestamp'])) * 1e9)
        rosmsg.header.frame_id = sms_msg['frame_id']
        rosmsg.group_fields = sms_msg['group_fields']
        rosmsg.vpestatus.attitude_quality = sms_msg['vpestatus']['attitude_quality']
        rosmsg.vpestatus.gyro_saturation = sms_msg['vpestatus']['gyro_saturation']
        rosmsg.vpestatus.gyro_saturation_recovery = sms_msg['vpestatus']['gyro_saturation_recovery']
        rosmsg.vpestatus.mag_disturbance = sms_msg['vpestatus']['mag_disturbance']
        rosmsg.vpestatus.mag_saturation = sms_msg['vpestatus']['mag_saturation']
        rosmsg.vpestatus.acc_disturbance = sms_msg['vpestatus']['acc_disturbance']
        rosmsg.vpestatus.acc_saturation = sms_msg['vpestatus']['acc_saturation']
        rosmsg.vpestatus.known_mag_disturbance = sms_msg['vpestatus']['known_mag_disturbance']
        rosmsg.vpestatus.known_accel_disturbance = sms_msg['vpestatus']['known_accel_disturbance']
        rosmsg.yawpitchroll = Vector3(x = sms_msg['yawpitchroll']['x'], y=sms_msg['yawpitchroll']['y'], z= sms_msg['yawpitchroll']['z'])
        rosmsg.quaternion = Quaternion(x = sms_msg['quaternion']['x'], y=sms_msg['quaternion']['y'], z= sms_msg['quaternion']['z'], w = sms_msg['quaternion']['w'])
        rosmsg.dcm = sms_msg['dcm']
        rosmsg.magned = Vector3(x = sms_msg['magned']['x'], y=sms_msg['magned']['y'], z= sms_msg['magned']['z'])
        rosmsg.accelned = Vector3(x = sms_msg['accelned']['x'], y=sms_msg['accelned']['y'], z= sms_msg['accelned']['z'])
        rosmsg.linearaccelbody = Vector3(x = sms_msg['linearaccelbody']['x'], y=sms_msg['linearaccelbody']['y'], z= sms_msg['linearaccelbody']['z'])
        rosmsg.linearaccelned = Vector3(x = sms_msg['linearaccelned']['x'], y=sms_msg['linearaccelned']['y'], z= sms_msg['linearaccelned']['z'])
        rosmsg.ypru = Vector3(x = sms_msg['ypru']['x'], y=sms_msg['ypru']['y'], z= sms_msg['ypru']['z'])
        
        self.vectornav_raw_attitude_pub.publish(rosmsg)

    def vectornav_raw_common_callback(self, sms_msg):
        rosmsg = CommonGroup()
        rosmsg.header.stamp.sec = int(sms_msg['timestamp'])
        rosmsg.header.stamp.nanosec = int((sms_msg['timestamp']-int(sms_msg['timestamp'])) * 1e9)
        rosmsg.header.frame_id = sms_msg['frame_id']
        rosmsg.group_fields = sms_msg['group_fields']
        rosmsg.timestartup = sms_msg['timestartup']
        rosmsg.timegps = sms_msg['timegps']
        rosmsg.timesyncin = sms_msg['timesyncin']
        rosmsg.yawpitchroll = Vector3(x = sms_msg['yawpitchroll']['x'], y=sms_msg['yawpitchroll']['y'], z= sms_msg['yawpitchroll']['z'])
        rosmsg.quaternion = Quaternion(x = sms_msg['quaternion']['x'], y=sms_msg['quaternion']['y'], z= sms_msg['quaternion']['z'], w = sms_msg['quaternion']['w'])
        rosmsg.angularrate = Vector3(x = sms_msg['angularrate']['x'], y=sms_msg['angularrate']['y'], z= sms_msg['angularrate']['z'])
        rosmsg.position = Vector3(x = sms_msg['position']['x'], y=sms_msg['position']['y'], z= sms_msg['position']['z'])
        rosmsg.velocity = Vector3(x = sms_msg['velocity']['x'], y=sms_msg['velocity']['y'], z= sms_msg['velocity']['z'])
        rosmsg.accel = Vector3(x = sms_msg['accel']['x'], y=sms_msg['accel']['y'], z= sms_msg['accel']['z'])
        rosmsg.imu_accel = Vector3(x = sms_msg['imu_accel']['x'], y=sms_msg['imu_accel']['y'], z= sms_msg['imu_accel']['z'])
        rosmsg.imu_rate = Vector3(x = sms_msg['imu_rate']['x'], y=sms_msg['imu_rate']['y'], z= sms_msg['imu_rate']['z'])
        rosmsg.magpres_mag = Vector3(x = sms_msg['magpres_mag']['x'], y=sms_msg['magpres_mag']['y'], z= sms_msg['magpres_mag']['z'])
        rosmsg.magpres_temp = sms_msg['magpres_temp']
        rosmsg.magpres_pres = sms_msg['magpres_pres']
        rosmsg.deltatheta_dtime = sms_msg['deltatheta_dtime']
        rosmsg.deltatheta_dtheta = Vector3(x = sms_msg['deltatheta_dtheta']['x'], y=sms_msg['deltatheta_dtheta']['y'], z= sms_msg['deltatheta_dtheta']['z'])
        rosmsg.deltatheta_dvel = Vector3(x = sms_msg['deltatheta_dvel']['x'], y=sms_msg['deltatheta_dvel']['y'], z= sms_msg['deltatheta_dvel']['z'])
        rosmsg.insstatus.mode = sms_msg['insstatus']['mode']
        rosmsg.insstatus.gps_fix = sms_msg['insstatus']['gps_fix']
        rosmsg.insstatus.time_error = sms_msg['insstatus']['time_error']
        rosmsg.insstatus.imu_error = sms_msg['insstatus']['imu_error']
        rosmsg.insstatus.mag_pres_error = sms_msg['insstatus']['mag_pres_error']
        rosmsg.insstatus.gps_error = sms_msg['insstatus']['gps_error']
        rosmsg.insstatus.gps_heading_ins = sms_msg['insstatus']['gps_heading_ins']
        rosmsg.insstatus.gps_compass = sms_msg['insstatus']['gps_compass']
        rosmsg.syncincnt = sms_msg['syncincnt']
        rosmsg.timegpspps = sms_msg['timegpspps']

        self.vectornav_raw_common_pub.publish(rosmsg)
    
    def vectornav_raw_gps_callback(self, sms_msg):
        rosmsg = GpsGroup()
        rosmsg.header.stamp.sec = int(sms_msg['timestamp'])
        rosmsg.header.stamp.nanosec = int((sms_msg['timestamp']-int(sms_msg['timestamp'])) * 1e9)
        rosmsg.header.frame_id = sms_msg['frame_id']
        rosmsg.group_fields = sms_msg['group_fields']
        rosmsg.utc.year = sms_msg['utc']['year']
        rosmsg.utc.month = sms_msg['utc']['month']
        rosmsg.utc.day = sms_msg['utc']['day']
        rosmsg.utc.hour = sms_msg['utc']['hour']
        rosmsg.utc.min = sms_msg['utc']['min']
        rosmsg.utc.sec = sms_msg['utc']['sec']
        rosmsg.utc.ms = sms_msg['utc']['ms']
        rosmsg.tow = sms_msg['tow']
        rosmsg.week = sms_msg['week']
        rosmsg.numsats = sms_msg['numsats']
        rosmsg.fix = sms_msg['fix']
        rosmsg.poslla = Vector3(x = sms_msg['poslla']['x'], y=sms_msg['poslla']['y'], z= sms_msg['poslla']['z'])
        rosmsg.posecef = Vector3(x = sms_msg['posecef']['x'], y=sms_msg['posecef']['y'], z= sms_msg['posecef']['z'])
        rosmsg.velned = Vector3(x = sms_msg['velned']['x'], y=sms_msg['velned']['y'], z= sms_msg['velned']['z'])
        rosmsg.velecef = Vector3(x = sms_msg['velecef']['x'], y=sms_msg['velecef']['y'], z= sms_msg['velecef']['z'])
        rosmsg.posu = Vector3(x = sms_msg['posu']['x'], y=sms_msg['posu']['y'], z= sms_msg['posu']['z'])
        rosmsg.velu = sms_msg['velu']
        rosmsg.timeu = sms_msg['timeu']
        rosmsg.timeinfo.timeinfo_status = sms_msg['timeinfo']['status']
        rosmsg.timeinfo.timeinfo_leapseconds = sms_msg['timeinfo']['leapseconds']
        rosmsg.dop.g = sms_msg['dop']['g']
        rosmsg.dop.p = sms_msg['dop']['p']
        rosmsg.dop.t = sms_msg['dop']['t']
        rosmsg.dop.v = sms_msg['dop']['v']
        rosmsg.dop.h = sms_msg['dop']['h']
        rosmsg.dop.n = sms_msg['dop']['n']
        rosmsg.dop.e = sms_msg['dop']['e']

        self.vectornav_raw_gps_pub.publish(rosmsg)

    def vectornav_raw_gps2_callback(self, sms_msg):
        rosmsg = GpsGroup()
        rosmsg.header.stamp.sec = int(sms_msg['timestamp'])
        rosmsg.header.stamp.nanosec = int((sms_msg['timestamp']-int(sms_msg['timestamp'])) * 1e9)
        rosmsg.header.frame_id = sms_msg['frame_id']
        rosmsg.group_fields = sms_msg['group_fields']
        rosmsg.utc.year = sms_msg['utc']['year']
        rosmsg.utc.month = sms_msg['utc']['month']
        rosmsg.utc.day = sms_msg['utc']['day']
        rosmsg.utc.hour = sms_msg['utc']['hour']
        rosmsg.utc.min = sms_msg['utc']['min']
        rosmsg.utc.sec = sms_msg['utc']['sec']
        rosmsg.utc.ms = sms_msg['utc']['ms']
        rosmsg.tow = sms_msg['tow']
        rosmsg.week = sms_msg['week']
        rosmsg.numsats = sms_msg['numsats']
        rosmsg.fix = sms_msg['fix']
        rosmsg.poslla = Vector3(x = sms_msg['poslla']['x'], y=sms_msg['poslla']['y'], z= sms_msg['poslla']['z'])
        rosmsg.posecef = Vector3(x = sms_msg['posecef']['x'], y=sms_msg['posecef']['y'], z= sms_msg['posecef']['z'])
        rosmsg.velned = Vector3(x = sms_msg['velned']['x'], y=sms_msg['velned']['y'], z= sms_msg['velned']['z'])
        rosmsg.velecef = Vector3(x = sms_msg['velecef']['x'], y=sms_msg['velecef']['y'], z= sms_msg['velecef']['z'])
        rosmsg.posu = Vector3(x = sms_msg['posu']['x'], y=sms_msg['posu']['y'], z= sms_msg['posu']['z'])
        rosmsg.velu = sms_msg['velu']
        rosmsg.timeu = sms_msg['timeu']
        rosmsg.timeinfo.timeinfo_status = sms_msg['timeinfo']['status']
        rosmsg.timeinfo.timeinfo_leapseconds = sms_msg['timeinfo']['leapseconds']
        rosmsg.dop.g = sms_msg['dop']['g']
        rosmsg.dop.p = sms_msg['dop']['p']
        rosmsg.dop.t = sms_msg['dop']['t']
        rosmsg.dop.v = sms_msg['dop']['v']
        rosmsg.dop.h = sms_msg['dop']['h']
        rosmsg.dop.n = sms_msg['dop']['n']
        rosmsg.dop.e = sms_msg['dop']['e']

        self.vectornav_raw_gps2_pub.publish(rosmsg)

    def vectornav_raw_imu_callback(self, sms_msg):
        rosmsg = ImuGroup()
        rosmsg.header.stamp.sec = int(sms_msg['timestamp'])
        rosmsg.header.stamp.nanosec = int((sms_msg['timestamp']-int(sms_msg['timestamp'])) * 1e9)
        rosmsg.header.frame_id = sms_msg['frame_id']
        rosmsg.group_fields = sms_msg['group_fields']
        rosmsg.imustatus = sms_msg['imustatus']
        rosmsg.uncompmag = Vector3(x=sms_msg['uncompmag']['x'], y=sms_msg['uncompmag']['y'], z =sms_msg['uncompmag']['z'])
        rosmsg.uncompaccel = Vector3(x=sms_msg['uncompaccel']['x'], y=sms_msg['uncompaccel']['y'], z =sms_msg['uncompaccel']['z'])
        rosmsg.uncompgyro = Vector3(x=sms_msg['uncompgyro']['x'], y=sms_msg['uncompgyro']['y'], z =sms_msg['uncompgyro']['z'])
        rosmsg.temp = sms_msg['temp']
        rosmsg.pres = sms_msg['pres']
        rosmsg.deltatheta_time = sms_msg['deltatheta_time']
        rosmsg.deltatheta_dtheta = sms_msg['deltatheta_dtheta']
        rosmsg.deltavel = Vector3(x=sms_msg['deltavel']['x'], y=sms_msg['deltavel']['y'], z =sms_msg['deltavel']['z'])
        rosmsg.mag = Vector3(x=sms_msg['mag']['x'], y=sms_msg['mag']['y'], z =sms_msg['mag']['z'])
        rosmsg.accel = Vector3(x=sms_msg['accel']['x'], y=sms_msg['accel']['y'], z =sms_msg['accel']['z'])
        rosmsg.angularrate = Vector3(x=sms_msg['angularrate']['x'], y=sms_msg['angularrate']['y'], z =sms_msg['angularrate']['z'])
        rosmsg.sensat = sms_msg['sensat']

        self.vectornav_raw_imu_pub.publish(rosmsg)
        

if __name__ == '__main__':
    parser = argparse.ArgumentParser()
    parser.add_argument(
        '--config',
        type=str,
        default='default_params.json',
        help='SpireCV2 Config (.json)')
    parser.add_argument(
        '--job-name',
        type=str,
        default='live',
        help='SpireCV Job Name')
    parser.add_argument(
        '--ip',
        type=str,
        default='127.0.0.1',
        help='SpireMS Core IP')
    parser.add_argument(
        '--port',
        type=int,
        default=9094,
        help='SpireMS Core Port')
    args, unknown_args = parser.parse_known_args()
    if not os.path.isabs(args.config):
        current_path = os.path.abspath(__file__)
        params_dir = os.path.join(current_path[:current_path.find('spirecv-pro') + 11], 'params', 'spirecv2')
        args.config = os.path.join(params_dir, args.config)
    print("--config:", args.config)
    print("--job-name:", args.job_name)
    extra = get_extra_args(unknown_args)

    rclpy.init()
    node = SMSA2RLTopic2ROS2Node(args.job_name, param_dict_or_file=args.config, ip=args.ip, port=args.port, **extra)
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

